/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                        Intel License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of Intel Corporation may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/

/*
 * cvhaarclassifier.cpp
 *
 * haar classifiers (stump, CART, stage, cascade)
 */

#include "_cvhaartraining.h"


CvIntHaarClassifier* icvCreateCARTHaarClassifier( int count ) {
	CvCARTHaarClassifier* cart;
	size_t datasize;

	datasize = sizeof( *cart ) +
			   ( sizeof( int ) +
				 sizeof( CvTHaarFeature ) + sizeof( CvFastHaarFeature ) +
				 sizeof( float ) + sizeof( int ) + sizeof( int ) ) * count +
			   sizeof( float ) * (count + 1);

	cart = (CvCARTHaarClassifier*) cvAlloc( datasize );
	memset( cart, 0, datasize );

	cart->feature = (CvTHaarFeature*) (cart + 1);
	cart->fastfeature = (CvFastHaarFeature*) (cart->feature + count);
	cart->threshold = (float*) (cart->fastfeature + count);
	cart->left = (int*) (cart->threshold + count);
	cart->right = (int*) (cart->left + count);
	cart->val = (float*) (cart->right + count);
	cart->compidx = (int*) (cart->val + count + 1 );
	cart->count = count;
	cart->eval = icvEvalCARTHaarClassifier;
	cart->save = icvSaveCARTHaarClassifier;
	cart->release = icvReleaseHaarClassifier;

	return (CvIntHaarClassifier*) cart;
}


void icvReleaseHaarClassifier( CvIntHaarClassifier** classifier ) {
	cvFree( classifier );
	*classifier = NULL;
}


void icvInitCARTHaarClassifier( CvCARTHaarClassifier* carthaar, CvCARTClassifier* cart,
								CvIntHaarFeatures* intHaarFeatures ) {
	int i;

	for ( i = 0; i < cart->count; i++ ) {
		carthaar->feature[i] = intHaarFeatures->feature[cart->compidx[i]];
		carthaar->fastfeature[i] = intHaarFeatures->fastfeature[cart->compidx[i]];
		carthaar->threshold[i] = cart->threshold[i];
		carthaar->left[i] = cart->left[i];
		carthaar->right[i] = cart->right[i];
		carthaar->val[i] = cart->val[i];
		carthaar->compidx[i] = cart->compidx[i];
	}
	carthaar->count = cart->count;
	carthaar->val[cart->count] = cart->val[cart->count];
}


float icvEvalCARTHaarClassifier( CvIntHaarClassifier* classifier,
								 sum_type* sum, sum_type* tilted, float normfactor ) {
	int idx = 0;

	do {
		if ( cvEvalFastHaarFeature(
					((CvCARTHaarClassifier*) classifier)->fastfeature + idx, sum, tilted )
				< (((CvCARTHaarClassifier*) classifier)->threshold[idx] * normfactor) ) {
			idx = ((CvCARTHaarClassifier*) classifier)->left[idx];
		} else {
			idx = ((CvCARTHaarClassifier*) classifier)->right[idx];
		}
	} while ( idx > 0 );

	return ((CvCARTHaarClassifier*) classifier)->val[-idx];
}


CvIntHaarClassifier* icvCreateStageHaarClassifier( int count, float threshold ) {
	CvStageHaarClassifier* stage;
	size_t datasize;

	datasize = sizeof( *stage ) + sizeof( CvIntHaarClassifier* ) * count;
	stage = (CvStageHaarClassifier*) cvAlloc( datasize );
	memset( stage, 0, datasize );

	stage->count = count;
	stage->threshold = threshold;
	stage->classifier = (CvIntHaarClassifier**) (stage + 1);

	stage->eval = icvEvalStageHaarClassifier;
	stage->save = icvSaveStageHaarClassifier;
	stage->release = icvReleaseStageHaarClassifier;

	return (CvIntHaarClassifier*) stage;
}


void icvReleaseStageHaarClassifier( CvIntHaarClassifier** classifier ) {
	int i;

	for ( i = 0; i < ((CvStageHaarClassifier*) *classifier)->count; i++ ) {
		if ( ((CvStageHaarClassifier*) *classifier)->classifier[i] != NULL ) {
			((CvStageHaarClassifier*) *classifier)->classifier[i]->release(
				&(((CvStageHaarClassifier*) *classifier)->classifier[i]) );
		}
	}

	cvFree( classifier );
	*classifier = NULL;
}


float icvEvalStageHaarClassifier( CvIntHaarClassifier* classifier,
								  sum_type* sum, sum_type* tilted, float normfactor ) {
	int i;
	float stage_sum;

	stage_sum = 0.0F;
	for ( i = 0; i < ((CvStageHaarClassifier*) classifier)->count; i++ ) {
		stage_sum +=
			((CvStageHaarClassifier*) classifier)->classifier[i]->eval(
				((CvStageHaarClassifier*) classifier)->classifier[i],
				sum, tilted, normfactor );
	}

	return stage_sum;
}


CvIntHaarClassifier* icvCreateCascadeHaarClassifier( int count ) {
	CvCascadeHaarClassifier* ptr;
	size_t datasize;

	datasize = sizeof( *ptr ) + sizeof( CvIntHaarClassifier* ) * count;
	ptr = (CvCascadeHaarClassifier*) cvAlloc( datasize );
	memset( ptr, 0, datasize );

	ptr->count = count;
	ptr->classifier = (CvIntHaarClassifier**) (ptr + 1);

	ptr->eval = icvEvalCascadeHaarClassifier;
	ptr->save = NULL;
	ptr->release = icvReleaseCascadeHaarClassifier;

	return (CvIntHaarClassifier*) ptr;
}


void icvReleaseCascadeHaarClassifier( CvIntHaarClassifier** classifier ) {
	int i;

	for ( i = 0; i < ((CvCascadeHaarClassifier*) *classifier)->count; i++ ) {
		if ( ((CvCascadeHaarClassifier*) *classifier)->classifier[i] != NULL ) {
			((CvCascadeHaarClassifier*) *classifier)->classifier[i]->release(
				&(((CvCascadeHaarClassifier*) *classifier)->classifier[i]) );
		}
	}

	cvFree( classifier );
	*classifier = NULL;
}


float icvEvalCascadeHaarClassifier( CvIntHaarClassifier* classifier,
									sum_type* sum, sum_type* tilted, float normfactor ) {
	int i;

	for ( i = 0; i < ((CvCascadeHaarClassifier*) classifier)->count; i++ ) {
		if ( ((CvCascadeHaarClassifier*) classifier)->classifier[i]->eval(
					((CvCascadeHaarClassifier*) classifier)->classifier[i],
					sum, tilted, normfactor )
				< ( ((CvStageHaarClassifier*)
					 ((CvCascadeHaarClassifier*) classifier)->classifier[i])->threshold
					- CV_THRESHOLD_EPS) ) {
			return 0.0;
		}
	}

	return 1.0;
}


void icvSaveHaarFeature( CvTHaarFeature* feature, FILE* file ) {
	fprintf( file, "%d\n", ( ( feature->rect[2].weight == 0.0F ) ? 2 : 3) );
	fprintf( file, "%d %d %d %d %d %d\n",
			 feature->rect[0].r.x,
			 feature->rect[0].r.y,
			 feature->rect[0].r.width,
			 feature->rect[0].r.height,
			 0,
			 (int) (feature->rect[0].weight) );
	fprintf( file, "%d %d %d %d %d %d\n",
			 feature->rect[1].r.x,
			 feature->rect[1].r.y,
			 feature->rect[1].r.width,
			 feature->rect[1].r.height,
			 0,
			 (int) (feature->rect[1].weight) );
	if ( feature->rect[2].weight != 0.0F ) {
		fprintf( file, "%d %d %d %d %d %d\n",
				 feature->rect[2].r.x,
				 feature->rect[2].r.y,
				 feature->rect[2].r.width,
				 feature->rect[2].r.height,
				 0,
				 (int) (feature->rect[2].weight) );
	}
	fprintf( file, "%s\n", &(feature->desc[0]) );
}


void icvLoadHaarFeature( CvTHaarFeature* feature, FILE* file ) {
	int nrect;
	int j;
	int tmp;
	int weight;

	nrect = 0;
	fscanf( file, "%d", &nrect );

	assert( nrect <= CV_HAAR_FEATURE_MAX );

	for ( j = 0; j < nrect; j++ ) {
		fscanf( file, "%d %d %d %d %d %d",
				&(feature->rect[j].r.x),
				&(feature->rect[j].r.y),
				&(feature->rect[j].r.width),
				&(feature->rect[j].r.height),
				&tmp, &weight );
		feature->rect[j].weight = (float) weight;
	}
	for ( j = nrect; j < CV_HAAR_FEATURE_MAX; j++ ) {
		feature->rect[j].r.x = 0;
		feature->rect[j].r.y = 0;
		feature->rect[j].r.width = 0;
		feature->rect[j].r.height = 0;
		feature->rect[j].weight = 0.0f;
	}
	fscanf( file, "%s", &(feature->desc[0]) );
	feature->tilted = ( feature->desc[0] == 't' );
}


void icvSaveCARTHaarClassifier( CvIntHaarClassifier* classifier, FILE* file ) {
	int i;
	int count;

	count = ((CvCARTHaarClassifier*) classifier)->count;
	fprintf( file, "%d\n", count );
	for ( i = 0; i < count; i++ ) {
		icvSaveHaarFeature( &(((CvCARTHaarClassifier*) classifier)->feature[i]), file );
		fprintf( file, "%e %d %d\n",
				 ((CvCARTHaarClassifier*) classifier)->threshold[i],
				 ((CvCARTHaarClassifier*) classifier)->left[i],
				 ((CvCARTHaarClassifier*) classifier)->right[i] );
	}
	for ( i = 0; i <= count; i++ ) {
		fprintf( file, "%e ", ((CvCARTHaarClassifier*) classifier)->val[i] );
	}
	fprintf( file, "\n" );
}


CvIntHaarClassifier* icvLoadCARTHaarClassifier( FILE* file, int step ) {
	CvCARTHaarClassifier* ptr;
	int i;
	int count;

	ptr = NULL;
	fscanf( file, "%d", &count );
	if ( count > 0 ) {
		ptr = (CvCARTHaarClassifier*) icvCreateCARTHaarClassifier( count );
		for ( i = 0; i < count; i++ ) {
			icvLoadHaarFeature( &(ptr->feature[i]), file );
			fscanf( file, "%f %d %d", &(ptr->threshold[i]), &(ptr->left[i]),
					&(ptr->right[i]) );
		}
		for ( i = 0; i <= count; i++ ) {
			fscanf( file, "%f", &(ptr->val[i]) );
		}
		icvConvertToFastHaarFeature( ptr->feature, ptr->fastfeature, ptr->count, step );
	}

	return (CvIntHaarClassifier*) ptr;
}


void icvSaveStageHaarClassifier( CvIntHaarClassifier* classifier, FILE* file ) {
	int count;
	int i;
	float threshold;

	count = ((CvStageHaarClassifier*) classifier)->count;
	fprintf( file, "%d\n", count );
	for ( i = 0; i < count; i++ ) {
		((CvStageHaarClassifier*) classifier)->classifier[i]->save(
			((CvStageHaarClassifier*) classifier)->classifier[i], file );
	}

	threshold = ((CvStageHaarClassifier*) classifier)->threshold;

	/* to be compatible with the previous implementation */
	/* threshold = 2.0F * ((CvStageHaarClassifier*) classifier)->threshold - count; */

	fprintf( file, "%e\n", threshold );
}



CvIntHaarClassifier* icvLoadCARTStageHaarClassifierF( FILE* file, int step ) {
	CvStageHaarClassifier* ptr = NULL;

	//CV_FUNCNAME( "icvLoadCARTStageHaarClassifierF" );

	__BEGIN__;

	if ( file != NULL ) {
		int count;
		int i;
		float threshold;

		count = 0;
		fscanf( file, "%d", &count );
		if ( count > 0 ) {
			ptr = (CvStageHaarClassifier*) icvCreateStageHaarClassifier( count, 0.0F );
			for ( i = 0; i < count; i++ ) {
				ptr->classifier[i] = icvLoadCARTHaarClassifier( file, step );
			}

			fscanf( file, "%f", &threshold );

			ptr->threshold = threshold;
			/* to be compatible with the previous implementation */
			/* ptr->threshold = 0.5F * (threshold + count); */
		}
		if ( feof( file ) ) {
			ptr->release( (CvIntHaarClassifier**) &ptr );
			ptr = NULL;
		}
	}

	__END__;

	return (CvIntHaarClassifier*) ptr;
}


CvIntHaarClassifier* icvLoadCARTStageHaarClassifier( const char* filename, int step ) {
	CvIntHaarClassifier* ptr = NULL;

	CV_FUNCNAME( "icvLoadCARTStageHaarClassifier" );

	__BEGIN__;

	FILE* file;

	file = fopen( filename, "r" );
	if ( file ) {
		CV_CALL( ptr = icvLoadCARTStageHaarClassifierF( file, step ) );
		fclose( file );
	}

	__END__;

	return ptr;
}

/* tree cascade classifier */

/* evaluates a tree cascade classifier */

float icvEvalTreeCascadeClassifier( CvIntHaarClassifier* classifier,
									sum_type* sum, sum_type* tilted, float normfactor ) {
	CvTreeCascadeNode* ptr;

	ptr = ((CvTreeCascadeClassifier*) classifier)->root;

	while ( ptr ) {
		if ( ptr->stage->eval( (CvIntHaarClassifier*) ptr->stage,
							   sum, tilted, normfactor )
				>= ptr->stage->threshold - CV_THRESHOLD_EPS ) {
			ptr = ptr->child;
		} else {
			while ( ptr && ptr->next == NULL ) { ptr = ptr->parent; }
			if ( ptr == NULL ) { return 0.0F; }
			ptr = ptr->next;
		}
	}

	return 1.0F;
}

/* sets path int the tree form the root to the leaf node */

void icvSetLeafNode( CvTreeCascadeClassifier* tcc, CvTreeCascadeNode* leaf ) {
	CV_FUNCNAME( "icvSetLeafNode" );

	__BEGIN__;

	CvTreeCascadeNode* ptr;

	ptr = NULL;
	while ( leaf ) {
		leaf->child_eval = ptr;
		ptr = leaf;
		leaf = leaf->parent;
	}

	leaf = tcc->root;
	while ( leaf && leaf != ptr ) { leaf = leaf->next; }
	if ( leaf != ptr ) {
		CV_ERROR( CV_StsError, "Invalid tcc or leaf node." );
	}

	tcc->root_eval = ptr;

	__END__;
}

/* evaluates a tree cascade classifier. used in filtering */

float icvEvalTreeCascadeClassifierFilter( CvIntHaarClassifier* classifier, sum_type* sum,
		sum_type* tilted, float normfactor ) {
	CvTreeCascadeNode* ptr;
	CvTreeCascadeClassifier* tree;

	tree = (CvTreeCascadeClassifier*) classifier;



	ptr = ((CvTreeCascadeClassifier*) classifier)->root_eval;
	while ( ptr ) {
		if ( ptr->stage->eval( (CvIntHaarClassifier*) ptr->stage,
							   sum, tilted, normfactor )
				< ptr->stage->threshold - CV_THRESHOLD_EPS ) {
			return 0.0F;
		}
		ptr = ptr->child_eval;
	}

	return 1.0F;
}

/* creates tree cascade node */

CvTreeCascadeNode* icvCreateTreeCascadeNode() {
	CvTreeCascadeNode* ptr = NULL;

	CV_FUNCNAME( "icvCreateTreeCascadeNode" );

	__BEGIN__;
	size_t data_size;

	data_size = sizeof( *ptr );
	CV_CALL( ptr = (CvTreeCascadeNode*) cvAlloc( data_size ) );
	memset( ptr, 0, data_size );

	__END__;

	return ptr;
}

/* releases all tree cascade nodes accessible via links */

void icvReleaseTreeCascadeNodes( CvTreeCascadeNode** node ) {
	//CV_FUNCNAME( "icvReleaseTreeCascadeNodes" );

	__BEGIN__;

	if ( node && *node ) {
		CvTreeCascadeNode* ptr;
		CvTreeCascadeNode* ptr_;

		ptr = *node;

		while ( ptr ) {
			while ( ptr->child ) { ptr = ptr->child; }

			if ( ptr->stage ) { ptr->stage->release( (CvIntHaarClassifier**) &ptr->stage ); }
			ptr_ = ptr;

			while ( ptr && ptr->next == NULL ) { ptr = ptr->parent; }
			if ( ptr ) { ptr = ptr->next; }

			cvFree( &ptr_ );
		}
	}

	__END__;
}


/* releases tree cascade classifier */

void icvReleaseTreeCascadeClassifier( CvIntHaarClassifier** classifier ) {
	if ( classifier && *classifier ) {
		icvReleaseTreeCascadeNodes( &((CvTreeCascadeClassifier*) *classifier)->root );
		cvFree( classifier );
		*classifier = NULL;
	}
}


void icvPrintTreeCascade( CvTreeCascadeNode* root ) {
	//CV_FUNCNAME( "icvPrintTreeCascade" );

	__BEGIN__;

	CvTreeCascadeNode* node;
	CvTreeCascadeNode* n;
	char buf0[256];
	char buf[256];
	int level;
	int i;
	int max_level;

	node = root;
	level = max_level = 0;
	while ( node ) {
		while ( node->child ) { node = node->child; level++; }
		if ( level > max_level ) { max_level = level; }
		while ( node && !node->next ) { node = node->parent; level--; }
		if ( node ) { node = node->next; }
	}

	printf( "\nTree Classifier\n" );
	printf( "Stage\n" );
	for ( i = 0; i <= max_level; i++ ) { printf( "+---" ); }
	printf( "+\n" );
	for ( i = 0; i <= max_level; i++ ) { printf( "|%3d", i ); }
	printf( "|\n" );
	for ( i = 0; i <= max_level; i++ ) { printf( "+---" ); }
	printf( "+\n\n" );

	node = root;

	buf[0] = 0;
	while ( node ) {
		sprintf( buf + strlen( buf ), "%3d", node->idx );
		while ( node->child ) {
			node = node->child;
			sprintf( buf + strlen( buf ),
					 ((node->idx < 10) ? "---%d" : ((node->idx < 100) ? "--%d" : "-%d")),
					 node->idx );
		}
		printf( " %s\n", buf );

		while ( node && !node->next ) { node = node->parent; }
		if ( node ) {
			node = node->next;

			n = node->parent;
			buf[0] = 0;
			while ( n ) {
				if ( n->next ) {
					sprintf( buf0, "  | %s", buf );
				} else {
					sprintf( buf0, "    %s", buf );
				}
				strcpy( buf, buf0 );
				n = n->parent;
			}
			printf( " %s  |\n", buf );
		}
	}
	printf( "\n" );
	fflush( stdout );

	__END__;
}



CvIntHaarClassifier* icvLoadTreeCascadeClassifier( const char* filename, int step,
		int* splits ) {
	CvTreeCascadeClassifier* ptr = NULL;
	CvTreeCascadeNode** nodes = NULL;

	CV_FUNCNAME( "icvLoadTreeCascadeClassifier" );

	__BEGIN__;

	size_t data_size;
	CvStageHaarClassifier* stage;
	char stage_name[PATH_MAX];
	char* suffix;
	int i, num;
	FILE* f;
	int result, parent = 0, next = 0;
	int stub;

	if ( !splits ) { splits = &stub; }

	*splits = 0;

	data_size = sizeof( *ptr );

	CV_CALL( ptr = (CvTreeCascadeClassifier*) cvAlloc( data_size ) );
	memset( ptr, 0, data_size );

	ptr->eval = icvEvalTreeCascadeClassifier;
	ptr->release = icvReleaseTreeCascadeClassifier;

	sprintf( stage_name, "%s/", filename );
	suffix = stage_name + strlen( stage_name );

	for ( i = 0; ; i++ ) {
		sprintf( suffix, "%d/%s", i, CV_STAGE_CART_FILE_NAME );
		f = fopen( stage_name, "r" );
		if ( !f ) { break; }
		fclose( f );
	}
	num = i;

	if ( num < 1 ) { EXIT; }

	data_size = sizeof( *nodes ) * num;
	CV_CALL( nodes = (CvTreeCascadeNode**) cvAlloc( data_size ) );

	for ( i = 0; i < num; i++ ) {
		sprintf( suffix, "%d/%s", i, CV_STAGE_CART_FILE_NAME );
		f = fopen( stage_name, "r" );
		CV_CALL( stage = (CvStageHaarClassifier*)
						 icvLoadCARTStageHaarClassifierF( f, step ) );

		result = ( f && stage ) ? fscanf( f, "%d%d", &parent, &next ) : 0;
		if ( f ) { fclose( f ); }

		if ( result != 2 ) {
			num = i;
			break;
		}

		printf( "Stage %d loaded\n", i );

		if ( parent >= i || (next != -1 && next != i + 1) ) {
			CV_ERROR( CV_StsError, "Invalid tree links" );
		}

		CV_CALL( nodes[i] = icvCreateTreeCascadeNode() );
		nodes[i]->stage = stage;
		nodes[i]->idx = i;
		nodes[i]->parent = (parent != -1 ) ? nodes[parent] : NULL;
		nodes[i]->next = ( next != -1 ) ? nodes[i] : NULL;
		nodes[i]->child = NULL;
	}
	for ( i = 0; i < num; i++ ) {
		if ( nodes[i]->next ) {
			(*splits)++;
			nodes[i]->next = nodes[i+1];
		}
		if ( nodes[i]->parent && nodes[i]->parent->child == NULL ) {
			nodes[i]->parent->child = nodes[i];
		}
	}
	ptr->root = nodes[0];
	ptr->next_idx = num;

	__END__;

	cvFree( &nodes );

	return (CvIntHaarClassifier*) ptr;
}


CvTreeCascadeNode* icvFindDeepestLeaves( CvTreeCascadeClassifier* tcc ) {
	CvTreeCascadeNode* leaves;

	//CV_FUNCNAME( "icvFindDeepestLeaves" );

	__BEGIN__;

	int level, cur_level;
	CvTreeCascadeNode* ptr;
	CvTreeCascadeNode* last;

	leaves = last = NULL;

	ptr = tcc->root;
	level = -1;
	cur_level = 0;

	/* find leaves with maximal level */
	while ( ptr ) {
		if ( ptr->child ) { ptr = ptr->child; cur_level++; }
		else {
			if ( cur_level == level ) {
				last->next_same_level = ptr;
				ptr->next_same_level = NULL;
				last = ptr;
			}
			if ( cur_level > level ) {
				level = cur_level;
				leaves = last = ptr;
				ptr->next_same_level = NULL;
			}
			while ( ptr && ptr->next == NULL ) { ptr = ptr->parent; cur_level--; }
			if ( ptr ) { ptr = ptr->next; }
		}
	}

	__END__;

	return leaves;
}

/* End of file. */
